#include "Camera.h"
#include "Util.h"

static UINT g_Index = 0;

#define DEFAULT_FOCAL_LENGTH 532.0
#define DEFAULT_FOCAL_WEIGHT 0.0001
#define DEFUALT_DISTORATION_WEIGHT 100

CCamera::CCamera(CCameraMesh* pMesh) : m_pMesh(pMesh)
{
	InitExtrinsicParameters();
	VISIO_ASSERT(m_pMesh);
}

CCamera::CCamera(CCameraMesh* pMesh, cv::Mat rotationMat, cv::Mat translationVector, cv::Mat Image, float f) : m_pMesh(pMesh) , m_Image(Image)
{
	InitExtrinsicParameters();
	VISIO_ASSERT(m_pMesh);
	SetExtrinsicParameters(rotationMat, translationVector);
	m_Focal = f;
}

CCamera::CCamera(CCameraMesh* pMesh, SCameraParams params, CImageData* pImageData) : m_pMesh(pMesh), m_pImageData(pImageData), 
																									 m_Image(pImageData->GetImage()) 
																											
{
	InitExtrinsicParameters();
	VISIO_ASSERT(m_pMesh);
	SetExtrinsicParameters(params.R, params.t);
	m_Params = params;

	
	m_Focal = CV_MAT(m_Params.K, 0, 0);
	ZeroMemory(m_Distoration, sizeof(m_Distoration));
	ZeroMemory(m_InvDistorationPolynomial, sizeof(m_InvDistorationPolynomial));
	m_InvDistorationPolynomial[1] = 1.0;

	InitConstraints();
}

void CCamera::InitConstraints()
{
	ZeroMemory(&m_Constraints, sizeof(SCameraConstriants));

	//constrain Focal and distoration params
	m_Constraints.bConstrianed[6] = true;
	m_Constraints.bConstrianed[7] = true;
	m_Constraints.bConstrianed[8] = true;

	m_Constraints.Constraints[6] = m_pImageData->GetFocal();

	m_Constraints.Weights[6] = DEFAULT_FOCAL_WEIGHT;
	m_Constraints.Weights[7] = DEFUALT_DISTORATION_WEIGHT;
	m_Constraints.Weights[8] = DEFUALT_DISTORATION_WEIGHT;
}

void CCamera::InitExtrinsicParameters()
{
	m_Index = g_Index++;
	m_RotationMatrix = EYE(4);
	m_TranslationMatrix = EYE(4);
	m_CameraCenterVector = ZEROS(4,1);
}

void CCamera::SetExtrinsicParameters(cv::Mat rotationMat, cv::Mat translationVector)
{
	// if the view transform was R(x_world)+t = x_cam
	// then the model will be transformed by = R'x_cam -R'*t
	cv::Mat rtag = rotationMat.t();
	cv::Mat t = -rtag*translationVector;

	//rotation expansion
	cv::Mat Mat4x4 = EYE(4);
	cv::Mat SubMat = Mat4x4(cv::Range(0,3), cv::Range(0,3));
	rtag.copyTo(SubMat);
	Mat4x4.copyTo(m_RotationMatrix);

	//translation expansion
	Mat4x4 = EYE(4);
	cv::Mat HTVector = Utils::ExpandToHVector(t);
	HTVector.copyTo(Mat4x4.col(3));
	m_CameraCenterVector = HTVector.clone();
	Mat4x4.copyTo(m_TranslationMatrix);
}

CCameraMeshInstance* CCamera::CreateMeshInstance()
{
	m_pInst = m_pMesh->CreateInstance(Utils::ConvertCVtoM3D(m_RotationMatrix.t()), Utils::ConvertCVtoM3D(m_TranslationMatrix), m_Image, m_Focal);
	VISIO_ASSERT(m_pInst);
	return m_pInst;
}

SCameraParams CCamera::GetParameters() const
{
	return m_Params;
}

void CCamera::PrintCamera() const
{
	CONSOLE("\n---------------\n");
	CONSOLE("Printing Camera: %d\n", m_Index);
	CONSOLE("Rotation:\n");
	Utils::PrintMat<double>(m_Params.R);
	CONSOLE("translation:\n");
	Utils::PrintMat<double>(m_Params.t);
	CONSOLE("intrin params:\n");
	Utils::PrintMat<double>(m_Params.K);
}

SCameraParams::SCameraParams(cv::Mat _R, cv::Mat _t, cv::Mat _K, cv::Mat _P)
{
	R = _R;
	t = _t;
	K = _K;
	P = _P;
}

SCameraParams::SCameraParams()
{
	R = EYE(3);
	t = INIT_MAT(1,3,0,0,0);
	K = EYE(3);
	P = INIT_MAT(3,4,0,0,0,0,0,0,0,0,0,0,0,0);
}

double* CCamera::GetRodirougsRotationAsArray() const
{
	cv::Mat rodVector;
	cv::Rodrigues(m_Params.R, rodVector);
	VISIO_ASSERT(m_Params.t.isContinuous());
	VISIO_ASSERT(0);
	return NULL;
}

double CCamera::GetFocal() const
{
	VISIO_ASSERT(m_Focal == CV_MAT(m_Params.K, 0, 0));
	return m_Focal;
}

//important use camera translation and cam center
void CCamera::Update(cv::Mat R, cv::Mat t, double* IntrinsincsParams)
{
	m_Params.R = R;
	m_Params.t = t;
	SetExtrinsicParameters(m_Params.R, m_Params.t);
	UpdateIntrin(IntrinsincsParams[0], IntrinsincsParams + 1);
}

UINT CCamera::GetImageWidth() const
{
	return m_Image.cols;
}

UINT CCamera::GetImageHeight() const
{
	return m_Image.rows;
}

void CCamera::UpdateInvDistoration(double k1, double k2, double Focal)
{
	double KDist[POLY_INVERSE_DEGREE] = { 0.0, 1.0, 0.0, k1, 0.0, k2 };
	double HalfWidth = 0.5 * GetImageWidth();
	double HalfHeight = 0.5 * GetImageHeight();
	double MaxRadius = sqrt(HalfWidth * HalfWidth + HalfHeight * HalfHeight) / Focal;

	Utils::InvertDistortion(6, 6, 0.0, MaxRadius, 
		KDist, m_InvDistorationPolynomial);
}

void CCamera::UpdateIntrin(double Focal, double* Distoration)
{
	UpdateFocalPoint(Focal);
	memcpy(m_Distoration, Distoration, sizeof(m_Distoration));
	UpdateInvDistoration(Distoration[0], Distoration[1], Focal);
}

void CCamera::UpdateFocalPoint(double Focal)
{
	m_Focal = Focal;
	double KDiag[] = {Focal , Focal, 1};
	m_Params.K = Utils::Diag(3, KDiag);
}

void CCamera::UseDefaulFocalLength()
{
	UpdateFocalPoint(m_pImageData->GetFocal());
}

cv::Point2d CCamera::Project(double* _p, bool bUseInstrinsic, bool bUndistort) const
{

	SCameraParams params = GetParameters();
	cv::Mat p = INIT_MAT_ARRAY(3,1, _p);

	cv::Mat Rp = params.R * p;
	cv::Mat Rp_t = Rp + params.t;
	if (bUseInstrinsic)
	{
		Rp_t = params.K * Rp_t;
	}
	
	cv::Point2d proj = Utils::Project3D(Rp_t);
	double factor = 1.0;
	if (bUndistort)
	{
		double rsq = ( Utils::Dot(proj, proj) ) / (GetFocal() * GetFocal());
		factor = 1 + GetDistorationParams(0) * rsq + GetDistorationParams(1) * rsq * rsq;
	}
	
	proj *= factor;

	return proj;
}

cv::Point2d CCamera::Project(cv::Point3d _p, bool bUseInstrinsic, bool bUndistort) const
{
	double p[] = { _p.x, _p.y, _p.z};
	return Project(p, bUseInstrinsic, bUndistort);
}

cv::Point2d CCamera::GetKeyPoint( UINT index ) const
{
	return m_pImageData->GetKeyPoint(index);
}

UINT CCamera::GetImageIndex() const
{
	return m_pImageData->GetImageIndex();
}

CTrackData* CCamera::GetTrackFromKeyPointIndex( UINT KeyPointIndex ) const
{
	for (VisablePointItr it = m_pImageData->VisablePointsBegin(); it != m_pImageData->VisablePointsEnd(); it++)
	{
		// the second is the keypoint index 
		if (it->second == KeyPointIndex)
		{
			return it->first;
		}
	}

	VISIO_ASSERT(0);
	return NULL;
}

CImageData* CCamera::GetImageData() const
{
	return m_pImageData;
}

cv::Mat CCamera::GetCameraCenter() const
{
	double* CamCenter = GetMatrixDataAsDouble(m_CameraCenterVector);
	return INIT_MAT(3,1, CamCenter[0], CamCenter[1], CamCenter[2]);
}

UINT CCamera::GetAddedIndex() const
{
	return m_Index;
}

void CCamera::MarkPoint( UINT KeyPointIndex, int mark )
{
	VISIO_ASSERT(mark >= 0);
	m_pImageData->MarkKeyPoint(KeyPointIndex, mark);
}

UINT CCamera::GetNumVisablePoints() const
{
	return m_pImageData->GetNumVisablePoints();
}

UINT CCamera::GetNumPointsProjected() const
{
	UINT NumProjected = 0;
	for (UINT KeyPointIndex = 0; KeyPointIndex < m_pImageData->GetNumKeyPoints(); KeyPointIndex++ )
	{
		
		if (m_pImageData->IsKeyPointMarked(KeyPointIndex))
		{
			NumProjected++;
		}
	}

	return NumProjected;
}

void CCamera::RemoveMark( UINT KeyPointIndex )
{
	m_pImageData->RemoveMark(KeyPointIndex);
}

double CCamera::GetDistorationParams( UINT index) const
{
	return m_Distoration[index];
}

const double* CCamera::GetDistorationParams() const
{
	return m_Distoration;
}

SCameraConstriants CCamera::GetConstraints() const
{
	return m_Constraints;
}
void CCamera::ResetDistortationPoly()
{
	ZeroMemory(m_InvDistorationPolynomial, sizeof(m_InvDistorationPolynomial));
	m_InvDistorationPolynomial[1] = 1.0;
}

cv::Mat CCamera::UndistortNormalizedPoint(cv::Mat p) const
{
	double r = norm(p);
	if (r == 0.0)
		return p;

	double t = 1.0;
	double a = 0.0;

	for (int i = 0; i < POLY_INVERSE_DEGREE; i++) {
		a += t * m_InvDistorationPolynomial[i];
		t = t * r;
	}

	double factor = a / r;

	return p * factor;
}

void CCamera::PrintCameraToFile(FILE* pFile) const
{
	VISIO_ASSERT(pFile);

	// camera format is 
	// intrin parameters f, k1, k2
	// 3x3 rotation matrix R
	// 3x1 translation vector t

	fprintf(pFile, "%d %.15e %.15e %.15e\n", m_pImageData->GetImageIndex(), m_Focal, m_Distoration[0], m_Distoration[1]);

	for (int i = 0; i < 3; i++)
	{
		fprintf(pFile, "%.15e %.15e %.15e\n", CV_MAT(m_Params.R, i , 0), CV_MAT(m_Params.R, i , 1), CV_MAT(m_Params.R, i , 2));
	}

	fprintf(pFile, "%.15e %.15e %.15e\n", CV_MAT(m_Params.t, 0, 0), CV_MAT(m_Params.t, 0, 1), CV_MAT(m_Params.t, 0, 2));
	
}

KeyColor CCamera::GetColorOfKeyPoint( UINT index ) const
{
	return m_pImageData->GetKeyPointColor(index);
}
